/***********************************************************************/
/** imageglobal.c 
    Contains image processing routines.
    
    author Birgit Graf,   UWA, 1998, modified 2000 (Mk3/4)
    author Thomas Braunl, UWA, 1998 (HSV)
    
    modified for RAS by Mattias Bratt
*/
/***********************************************************************/

#include "imageproc.h"
#include "helper.h"
#include "eyebot.h"
#include <stdio.h>

/** thresholds for comparison of colours to ballcolour */
int thresh = 5;
int thresh2 = 7;

/** white point multipliers for red gren blue**/
float wht_mult_red=1.0;
float wht_mult_green=1.0;
float wht_mult_blue=1.0;

/** colour space lookup table from 3x6bit RGB to hue **/
// Size in memory is 2^(3x5)=32K=LUTres
#define LUTres 32
BYTE rgb2hueLUT[LUTres][LUTres][LUTres]; 
int LUTinitialised =FALSE;




/** ball colour */
int BallColour = 42; //red is default


/***********************************************************************/
/** Init camera.

    Called by main().
*/
/***********************************************************************/

void Init_Cam()
{
  int camera;
  /*int bright, hue, sat;*/
/*  int i,x,y; */

  camera = CAMInit(NORMAL);

  if (camera < COLCAM)
    error("No colour camera!");
  else
  {
    if (camera == NOCAM)
      error("No camera!\n");
    else
      if (camera == INITERROR)
        error("CAMInit!\n");
  }
}

/***********************************************************************/
/** Change camera parameters.
    Changes brightness, hue and saturation in case default values
    aren't good enough (e.g. different light conditions). Function as
    in RoBiOS-Setup, but without camera-initialisation, works only for
    colour-camera.
*/
/***********************************************************************/

void set_cam_parameters ()
{
  image greyimg;
  colimage img;
  int fps,w,h;
  int end_proc = FALSE;

  LCDClear();
  LCDMenu("AUT","NAUT","","END");
  LCDPrintf("Camera param.:\n");

  while (!end_proc)
  {
    CAMGet(&fps, &w, &h);

    LCDSetPos(2, 0);
    LCDPrintf("FPS: %d\n", fps);
    LCDPrintf("FullWidth: %d\n", w);
    LCDPrintf("FullHeight: %d\n", h);

    CAMGetColFrame(&img, FALSE);
    IPColor2Grey(&img, &greyimg);

    LCDPutGraphic (&greyimg);

    switch (KEYRead())
    {
    case KEY1:    
      CAMMode(AUTOBRIGHTNESS);
       break;

    case KEY2:  
      CAMMode(NOAUTOBRIGHTNESS);
      break;

    case KEY4:
      LCDClear(); end_proc = TRUE; break;

    default: break;
    }
  }
}


/***********************************************************************/
/** Change parameters for image processing.
    Changes thresholds which are used to select ball/goal coloured
    pixels from others and size of ball/goal.

*/
/***********************************************************************/

void set_img_parameters()
{
  int ind = 0;
  int end_proc = FALSE;

  LCDClear();
  LCDMenu("CHG", "NXT", " ", "END");
      
  while (!end_proc)
  {
    LCDSetPos(0, 0);
    LCDPrintf("Image parameters\n");

    LCDSetPos(2, 0);
    LCDPrintf("Hue Thresh\n");
    LCDSetPos(3, 0);
    LCDPrintf("Hue Thresh2\n");

    LCDSetChar(2 + ind, 15, '*');

    switch (KEYGet())
    {
    case KEY1:
      switch(ind)
      {
      case 0:
        thresh = set_iparam("Hue Thresh", 0, thresh, 100, 1);
        break;
      case 1:
        thresh2 = set_iparam("Thresh ball", thresh, thresh2, 100, 1);
        break;
       default: break;
      }
      
      LCDMenu("CHG", "NXT", " ", "END");
      break;

    case KEY2:
      LCDSetChar(2 + ind, 15, ' ');
      ind ++;
  
      if (ind > 1) ind = 0;
  
      break;

    case KEY4:
      LCDClear(); end_proc = TRUE; break;

    default: break;
    }
  }
}

//forward declaration
void correct_colourB(BYTE *r,BYTE *g,BYTE *b);

/***********************************************************************/
/** Change RBG to HSV -- use hue only.
    Thomas Braunl, UWA 1998.
    
    param r,g,b rgb value of single pixel
    return hue for single RGB value 
*/
/***********************************************************************/

#define MIN(a,b) (a<b?a:b)
#define MAX(a,b) (a>b?a:b)

int RGBtoHue(BYTE r, BYTE g, BYTE b)
{
  BYTE hue /*,sat, val*/, delta, max, min;
  
  correct_colourB(&r,&g,&b);

  max   = MAX(r, MAX(g,b));
  min   = MIN(r, MIN(g,b));
  delta = max - min;
  hue =0;
  /* initialise hue*/
  
  /* val   = max;
     if (max != 0) sat = delta / max; else sat = 0;
     if (sat == 0) hue = NO_HUE;
  */
  if (2 * delta <= max) hue = NO_HUE;
  else {
    if (r == max) hue =  42 + 42*(g-b) / delta; /* 1*42 */
    else if  (g == max) hue = 126 + 42*(b-r) / delta; /* 3*42 */
    else if (b == max) hue = 210 + 42*(r-g) / delta; /* 5*42 */
    /* now: hue is in range [0..252] */
  }
  return hue;
}


/***********************************************************************/
/** Set target colour.
    Set target colour to colour detected in middle of picture
    (mean value of 5x5 area around middle of picture).

    param img colour picture
    return mean colour of image middle and surroundings
*/
/***********************************************************************/

int set_colour(colimage img)
{
  int row, column;
  int count = 0;
  int my_hue, hue;

  my_hue = 0;

  for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++)
    for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++)
    {
      hue = RGBtoHue(img[row][column][0], img[row][column][1],
         img[row][column][2]);      
      if (hue != NO_HUE)
      {
        my_hue += hue;
        count ++;
      } 
    }

  if (count != 0)
    my_hue /= count;

  LCDClear();
  LCDMenu("", "", "", "OK");
  LCDSetPos(7,0);
  LCDPrintf("HUE: %3d\n", my_hue);

  KEYWait(KEY4); LCDClear();

  return my_hue;
}


/***********************************************************************/
/** Set whitepoint.
    Set  colour detected in middle of picture
    (mean value of 5x5 area around middle of picture).
    to white by defining multipliers for R, G and B
    to make their adjusted values equal at this point.

*/
/***********************************************************************/

void set_white_point(colimage img)
{
  int row, column;
  int count = 0;
  int r=0,g=0,b=0;

  for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++)
    for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++)
    {
      r+=img[row][column][0];
      g+=img[row][column][1];
      b+=img[row][column][2];      
      count ++;
    }

  if (count != 0){
    r /= count;
    g /= count;
    b /= count;
  }
  int max = MAX(r, MAX(g,b));

  wht_mult_red=((float)max)/r;
  wht_mult_green=((float)max)/g;
  wht_mult_blue=((float)max)/b;

  LCDClear();
  LCDMenu("", "", "", "OK");
  LCDSetPos(2,0);
  LCDPrintf("r: %d\n", r);
  LCDSetPos(3,0);
  LCDPrintf("g: %d\n", g);
  LCDSetPos(4,0);
  LCDPrintf("b: %d\n", b);

  KEYWait(KEY4); LCDClear();
}


void reset_white_point()
{
  wht_mult_red=wht_mult_green=wht_mult_blue=1.0;
  LCDClear();
  LCDMenu("", "", "", "OK");
  LCDSetPos(2,0);
  LCDPrintf("White point");
  LCDSetPos(3,0);
  LCDPrintf("reset");
  KEYWait(KEY4); LCDClear();
}

/** correct colour using white point determined in set_white_point() **/

void correct_colour(int *r,int *g,int *b){
  int cr=wht_mult_red*(*r);
  int cg=wht_mult_green*(*g);
  int cb=wht_mult_blue*(*b);
  *r=*g=*b=255;
  if(cr<255) *r=cr;
  if(cg<255) *g=cg;
  if(cb<255) *b=cb;
}
void correct_colourB(BYTE *r,BYTE *g,BYTE *b){
  int cr=wht_mult_red*(*r);
  int cg=wht_mult_green*(*g);
  int cb=wht_mult_blue*(*b);
  *r=*g=*b=255;
  if(cr<255) *r=cr;
  if(cg<255) *g=cg;
  if(cb<255) *b=cb;
}

/** Compute values for the hue lookup table using
 ** values from RGBtuHUE, which compensates for
 ** chosen white point so that the LUT will also
 ** do so.
 **/

void compute_rgb2hueLUT(){
  BYTE r,g,b;
  LCDClear();
  LCDMenu("", "", "", "ABRT");
  LCDSetPos(2,0);
  LCDPrintf("Computing colour");
  LCDSetPos(3,0);
  LCDPrintf("lookup table");
  LCDSetPos(5,0);
  LCDPrintf("   of %d rows",LUTres);
  LCDSetPos(6,0);
  LCDPrintf("completed");

  for(r=0;r<LUTres;r++) {
    LCDSetPos(5,0);
    LCDPrintf("%2d",r);
    if(KEYRead()==KEY4) return;
    for(g=0;g<LUTres;g++){
      for(b=0;b<LUTres;b++){
	rgb2hueLUT[r][g][b]=RGBtoHue(r*(256/LUTres),g*(256/LUTres),b*(256/LUTres));
      }
    }
  }	
  LUTinitialised=TRUE;
}

/** Use fast lookup table method to find the hue
 ** from rgb values
 **/

int rgb2hue(int r, int g, int b){
  if(LUTinitialised)
    return rgb2hueLUT[r/(256/LUTres)][g/(256/LUTres)][b/(256/LUTres)];
  else
    return RGBtoHue(r,g,b);
}


/***********************************************************************/
/** Set Ball colour.

    Called by main().

*/
/***********************************************************************/

void set_ball_colour(colimage img)
{
  BallColour = set_colour(img);
}



/***********************************************************************/
/** Mark object.
    Convert colour picture into greyscale (for LCD output),
    mark ball/goal position by drawing a vertical and horizontal white
    line through it. Ball and it's size is displayed by black lines.
    
    Called by find_object().
    
    param row_middle, col_middle pixel-coordinates of object
    param object_size width of marking
*/
/***********************************************************************/

void mark_object(image greyimg, int row_middle, int col_middle, int object_size)
{
  int row, column;
  if (row_middle<1 || row_middle>=imagerows-1 || col_middle<1 || col_middle>=imagecolumns-1)
    error("mark_object: ");
  /* mark object position: WHITE */  
  for (row = 1; row < imagerows; row ++)
    greyimg[row][col_middle] = WHITE;
  
  for (column = 1; column < imagecolumns; column ++)
    greyimg[row_middle][column] = WHITE;

  
  /* mark object size: BLACK */
  int col1=col_middle - object_size/2;
  int col2=col_middle + object_size/2;

  for (row = row_middle - 7; row < row_middle + 8; row ++)
  {
    if(row<1||row>=imagerows-1) continue;
    if(col1>0 && col1<imagecolumns-1) greyimg[row][col1] = BLACK;
    if(col2>0 && col2<imagecolumns-1) greyimg[row][col2] = BLACK;
  }  

  for (column = col_middle - object_size / 2 + 1; column < col_middle +object_size / 2; column ++)
    if(column>0 && column<imagecolumns-1)
      greyimg[row_middle][column] = BLACK;
}


  
/***********************************************************************/
/** Search for coloured object.
    Search for reagions in rows, with consecutive matching pixels. 
    Hue error must be below thresh at the endpoints, but between them
    max_lowq_seq consecutive pixels are allowed to match less strictly (thresh2)
    or have undefined hue (NO_HUE).
    max_bad_in_seq pixels in this sequence that do not match at all are
    also allowed.

    param *row_middle, *column_middle pointers to pixel-coordinates of middle
    of object.
    param *object_size pointer to object's size in number of columns
    param *mean_error mean of the absolute hue error over the sequence
    param colour desired colour of searched object
    global thresh threshold for colour-comparison
    global thresh2 less strict (larger) threshold for colour-comparison
    return TRUE or FALSE if ball/goal was found or not
    return changed ball/goal coordinates, error  and size in referenced
    variables
*/
/***********************************************************************/


#define max_lowq_seq 4 //number of low quality (thresh2) matching pixels allowed in sequence between good matches 
#define max_bad_in_seq 1 //number of unmatching pixels allowed between good matches
#define min_seq_length 6 //minimum length of sequence == minimum size of object tha can be detected

int find_object(int *row_middle, int *col_middle, int *object_size, int *mean_error,
    int colour, colimage img) {
  // use short (16 bits) and BYTE (8bits unsigned) instead of int to save time

  BYTE row, column;
  BYTE count=0;            /* no.of pixels with valid hue */
  BYTE best_row=0, best_column=0, best_count=0, best_mean_diff=255;//best region found

  BYTE huediff;
  short sumdiff=0;
  short sumlowqdiff=0;
  short meandiff;
  BYTE hue;
  BYTE lowq_count=0; /*number of low quality matches since last good match*/
  BYTE bad_count=0; /*number of pixels not matching since last good match*/
  BYTE sequence_ended=FALSE;
    
  for (row = 1; row < imagerows-1; row ++) {
    for (column=1;column < imagecolumns-1;column++){
      hue=rgb2hue(img[row][column][0],img[row][column][1],img[row][column][2]);
      if(hue!=NO_HUE){
	huediff=colour-hue; if(huediff>127) huediff=-huediff; // abs(colour-hue)
        //if the diff is larger than 126 than the other
        //way round is shorter because max hue value is 253 (e.g. hue 2 is close to 252)
	if (huediff > 126) huediff = 253 - huediff; 
      }
      else huediff=NO_HUE;
 
      if(huediff<=thresh) {
	count++;
	sumdiff+=huediff;
	sumlowqdiff=0;
	lowq_count=0;
	bad_count=0;
      }
      else if(count>0) { 
	if(lowq_count<max_lowq_seq) {
	  if(huediff<= thresh2) {
	    count++;
	    lowq_count++;
	    sumdiff+=huediff;	  
	    sumlowqdiff+=huediff;	  
	  }
	  else if(huediff==NO_HUE) {
	    count++;
	    lowq_count++;
	    sumdiff+=thresh2;	  
	    sumlowqdiff+=thresh2;	  
	  }
	  else if(bad_count<max_bad_in_seq){
	    count++;
	    lowq_count++;
	    bad_count++;
	    sumdiff+=huediff;
	    sumlowqdiff+=huediff;	  
	  }
	  else sequence_ended=TRUE;
	}
	else  sequence_ended=TRUE;
	if(sequence_ended) {
	  sequence_ended=FALSE;
	  count-=lowq_count;
	  meandiff=(sumdiff-sumlowqdiff)/count;
	  if(count>min_seq_length&&meandiff<best_mean_diff) {
	    best_row=row;
	    best_column=column-count-lowq_count;
	    best_count=count;
	    best_mean_diff=meandiff;
	  }
	  count=0;
	  sumdiff=0;
	}
      }
    }
    if(count!=0){ // sequence ended at end of row
      count-=lowq_count;
      meandiff=(sumdiff-sumlowqdiff)/count;
      if(count>min_seq_length&&meandiff<best_mean_diff) {
	best_row=row;
	best_column=column-count-lowq_count;
	best_count=count;
	best_mean_diff=meandiff;
      }
      count=0;
      sumdiff=0;
    }
  }
  *row_middle=best_row;
  *col_middle=best_column+best_count/2;  
  *object_size=best_count;
  *mean_error=best_mean_diff;

  if(best_count>min_seq_length) return TRUE;
  else return FALSE;
}

/***********************************************************************/
/** Get ball colour.
    Get current values for ball colour.
    
    Called by main().
*/
/***********************************************************************/

int get_ball_col()
{
    return BallColour;
}

void get_colimage(colimage *img) {
  typedef BYTE bigcolimage[144][176][3];
  bigcolimage big_img;
  CAMGetFrameRGB((BYTE *)big_img);
  BYTE row,col,bigrow,bigcol;
  for(row=0,bigrow=10;row<imagerows;row++,bigrow+=1) {
    for(col=0,bigcol=6;col<imagecolumns;col++,bigcol+=1) {
      (*img)[row][col][0]=big_img[bigrow][bigcol][0];
      (*img)[row][col][1]=big_img[bigrow][bigcol][1];
      (*img)[row][col][2]=big_img[bigrow][bigcol][2];
    }
  }
}
void get_subsampled_colimage(colimage *img) {
  typedef BYTE bigcolimage[144][176][3];
  bigcolimage big_img;
  CAMGetFrameRGB((BYTE *)big_img);
  BYTE row,col,bigrow,bigcol;
  for(row=0,bigrow=10;row<imagerows;row++,bigrow+=2) {
    for(col=0,bigcol=6;col<imagecolumns;col++,bigcol+=2) {
      (*img)[row][col][0]=big_img[bigrow][bigcol][0];
      (*img)[row][col][1]=big_img[bigrow][bigcol][1];
      (*img)[row][col][2]=big_img[bigrow][bigcol][2];
    }
  }
}

void sendString(char* s )
{
  while (*s)
  {
    OSSendRS232( s, SERIAL1);
    s++;
  }
}
                 
void sendRGBData(colimage img)
{
  int i,j;
  char temp[100];
  OSInitRS232( SER38400, NONE, SERIAL1 );  
/* sprintf(temp,
	  "P3\n"
	  "82 62\n"
	  "255\n");
  sendString(temp);
  */
for(i=0, j=0; i < imagerows*imagecolumns; i++, j+=3)
  {
    BYTE r,g,b;
    r=((BYTE *)img)[j];
    g=((BYTE *)img)[j+1];
    b=((BYTE *)img)[j+2];
    correct_colourB(&r,&g,&b);
    sprintf(temp,"%u %u %u \n",r,g,b);
    if ((i % imagecolumns) == 0)
      {
	LCDClear();
	LCDPrintf(" %4d of %4d\n", i, imagerows*imagecolumns );
      }
	sendString(temp);
  }
  /* send RS232 termination character */
  sendString("\x04\n");
}                     

void sendGreyData(image img)
{
  int i,j;
  char temp[100];
  OSInitRS232( SER38400, NONE, SERIAL1 );  
  
  sprintf(temp,
	  "P2\n"
	  "82 62\n"
	  "15\n");
  sendString(temp);
  for(i=0, j=0; i < imagerows*imagecolumns; i++)
  {
    sprintf(temp,"%u\n",((BYTE *)img)[i]);
    if ((i % imagecolumns) == 0)
      {
	LCDClear();
	LCDPrintf(" %4d of %4d\n", i, imagerows*imagecolumns );
      }
    sendString(temp);
  }
  /* send RS232 termination character */
  sendString("\x04\n");
}

